#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=0;
double time_last = 0;
string frame_id_string_,pc_path_name_;
ros::Publisher cloud_pub;

void pcd2bin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, string out_file)
{
    pcl::io::savePCDFile(out_file, *cloud);
}

void cloudCB(const sensor_msgs::PointCloud2ConstPtr& pc_msg, const sensor_msgs::ImageConstPtr& front_image_msg){
    if( (pc_msg->header.stamp.toSec() - time_last )< 0.1){
        return;
    }
    time_last = pc_msg->header.stamp.toSec();

    ROS_INFO("cb ok");
    frame_id_string_=(__gnu_cxx::__to_xstring<string>(&std::vsnprintf, 4 * sizeof(int),
                                                      "%06d", frame_num_));
    pcl::PointCloud<PointType>::Ptr pc_tmp(new pcl::PointCloud<PointType>);//转成pcl点云
    pcl::fromROSMsg(*pc_msg, *pc_tmp);

   string tranin_or_test="training";
    // string tranin_or_test="testing";
    string dataset_name = "0119";
    cout<<tranin_or_test<<endl;
    cout<<frame_num_<<endl;

//    pc_path_name_="/home/st/ubuntu_data/dataset/nrsl/tj/"+dataset_name+"/"+tranin_or_test+"/velodyne/"+frame_id_string_+".pcd";
//    pcd2bin(pc_tmp,pc_path_name_);

   cv_bridge::CvImagePtr image_front;
   image_front = cv_bridge::toCvCopy(front_image_msg, sensor_msgs::image_encodings::BGR8);
   cv::imwrite("/media/st/data_fast/tj/"+dataset_name+"/"+tranin_or_test+"/image_2/"+frame_id_string_+".png",(*image_front).image);

 
    frame_num_++;
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;
//    tf2_ros::TransformListener tfl_(tfbuf_);
//    frame_ids_["odom"]=std::string("odom");
//    frame_ids_["base_link"]=std::string("base_link");
//    frame_ids_["map"]=std::string("map");
//    cloud_pub=nh.advertise<sensor_msgs::PointCloud2>("cloud_accum_convert",10,true);
//    ros::Subscriber cloud_sub=nh.subscribe("/cloud",10,cloudCB);



    message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh, "/velodyne_points", 1);
    message_filters::Subscriber<sensor_msgs::Image> image_front_sub(nh, "/camera/color/image_raw", 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,sensor_msgs::Image> MySyncPolicy;
    // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(20), pc_sub, image_front_sub);
    sync.setMaxIntervalDuration(ros::Duration(0.05));
    ROS_INFO("ok1");
    sync.registerCallback(boost::bind(&cloudCB, _1, _2));
    ROS_INFO("ok2");
    ros::spin();

    return 0;
}
